#pragma once

#include <list>

#include <rotor_swarm_msgs/msg/trajectory.hpp>
#include <nav_msgs/msg/path.hpp>
#include <rclcpp/rclcpp.hpp>

#include "trajectory_point.h"

namespace quadrotor_common
{

struct Trajectory
{
  Trajectory();
  Trajectory(const rotor_swarm_msgs::msg::Trajectory& trajectory_msg);
  Trajectory(const quadrotor_common::TrajectoryPoint& point);
  virtual ~Trajectory();

  rotor_swarm_msgs::msg::Trajectory toRosMessage() const;
  nav_msgs::msg::Path toRosPath() const;
  quadrotor_common::TrajectoryPoint getStateAtTime(
    const rclcpp::Duration& time_from_start) const;

  rclcpp::Time timestamp;

  enum class TrajectoryType
  {
    UNDEFINED, GENERAL, ACCELERATION, JERK, SNAP
  } trajectory_type;

  std::list<quadrotor_common::TrajectoryPoint> points;
};

} // namespace quadrotor_common
